#include "motor_drivers.hpp"



void MotorDrivers::MotorM3508::M3508Log(){
    logM3508.logWrite(std::to_string(std::chrono::duration_cast<std::chrono::milliseconds>
        (std::chrono::system_clock::now().time_since_epoch()).count()) + " - logM3508::");
    logM3508.logWrite(std::to_string(crtPos_) + " " + std::to_string(crtVel_) + " " + std::to_string(crtTorque_) + "\n");
    logM3508.logWrite(std::to_string(crtCycles_) + " " + std::to_string(crtOriginCycles_) + "\n");
}


void MotorDrivers::MotorM3508::motor_Update(){
    if(motor_getHWInfo()){
        switch(mode){
            case POS_MODE:
                pid_pos.target = targetPos_;
                pid_pos.IntegralLimit = static_cast<float>(targetPos_ + 20.0);
                pid_speed.target = (pid_pos.pidCalc(crtPos_) - targetPos_) / targetPos_
                    * posTargetVel_;

                pid_speed.IntegralLimit = posTargetVel_ + 20;
                targetVel_ = pid_speed.pidCalc(crtVel_);
            case SPEED_MODE:
                pid_speed.target = targetVel_;
                pid_speed.IntegralLimit = posTargetVel_ + 20;
                targetVel_ = pid_speed.pidCalc(crtVel_);
            default:
                return;
        }
    }

}

bool MotorDrivers::MotorM3508::motor_getHWInfo(){

    can_frame frame = socket_->socket_Read().can_frame_; //crt frame

    uint16_t p_int = static_cast<uint16_t>(frame.data[0] << 8) | frame.data[1];
    int16_t v_int = static_cast<int16_t>(frame.data[2] << 8) | frame.data[3];
    int16_t t_int = static_cast<int16_t>(frame.data[4] << 8) | frame.data[5];

    crtOriginCycles_ += socket_->socket_ReadMutation(0);

    while(crtOriginCycles_ >= 20){
        crtCycles_++;
        crtOriginCycles_ -= 20;
    }
    while(crtOriginCycles_ <= -20){
        crtCycles_--;
        crtOriginCycles_ += 20;
    }

    crtOriginPos_ = static_cast<float>(p_int / 8191.0 * 360.0);//degree
    crtPos_ = static_cast<float>(crtCycles_ + 
        (static_cast<float>(crtOriginCycles_) / 36.0));//cycle
    crtVel_ = static_cast<float>(v_int / 100.0);//rpm / 100.0
    crtTorque_ = static_cast<float>(t_int * 5.0 / 16384.0);//Nm

    M3508Log();
    
    return true;

}

MotorDrivers::M3508_Matrix::M3508_Matrix(std::vector<std::shared_ptr<MotorDrivers::MotorM3508>>* motors, 
MotorDrivers::HWSocket* s1, MotorDrivers::HWSocket* s2): s1_(s1), s2_(s2),motors_(motors){};

bool MotorDrivers::M3508_Matrix::motor_Init(){
    if(!s1_->socket_Init()) return false;
    if(!s2_->socket_Init()) return false;
    memset(disabled, 1, sizeof(disabled));
    for(auto const& m : *motors_){
        if(!m->motor_Init()) return false;
        int i = ((m->motor_GetSocket()->frame.can_frame_.can_id) ^ 0x200);
        disabled[i] = false;
    }
    return true;
}

void MotorDrivers::M3508_Matrix::motor_Update(){
    std::vector<std::thread> threads;

    for(size_t i = 0; i < motors_->size(); i++){
        if(!disabled[i]){
            // threads.push_back(std::thread([this, i](){
            motors_->at(i)->motor_Update();
            // }));
        }
    }
    // for(auto& thread : threads){
        // thread.join();
    // }
    uint8_t data[16];

    for(size_t i = 0; i < motors_->size(); i++){
        int16_t Anp = 0;
        if(!disabled[i]){
            Anp = static_cast<int16_t>(motors_->at(i)->targetVel_ / 500.0 * 12000.0);//
        }
        data[i] = (Anp >> 8);
        data[i+1] = Anp & 0xFF;
    }
    uint8_t data1[8]; std::copy(data, data + 8, data1);
    uint8_t data2[8]; std::copy(data + 8, data + 16, data2);
    s1_->socket_Send(data1);
    s2_->socket_Send(data2);
}
